
#include <chassis_controller/odometry_tricycle.h>
#include <boost/bind.hpp>



namespace chassis_controller
{
  TricycleOdometry::TricycleOdometry(std::shared_ptr<rclcpp_lifecycle::LifecycleNode> node):Odometry(node)
  {
    timestamp_ = node_->now();
  }
  // Sxya delta_pivot_pos;
  // Sxya delta_pos_in_vehicle;
  // Sxya pivot_vel_in_vehicle;

  void TricycleOdometry::init(const rclcpp::Time &time, std::vector<Spivot_value> &pivot_value)
  {
      // Reset accumulators and timestamp:
      timestamp_ = time;
      // init wheel pos
      int pivot_num = pivot_value.size();
      for (int i = 0; i < pivot_num; i++)
      {
      }
  }

  void TricycleOdometry::update(std::vector<Spivot_value> &pivot_value, std::vector<Spivot_param> &pivot_param , const rclcpp::Time &time, bool standstill,double odom_factor)
  {
    int pivot_num = pivot_value.size();
    if (pivot_num != 1)
    {
      DEBUG_OUT("num of pivot not equal 1");
      return;
    }

    //step1: calculate velocity and position of pivot
    if (pivot_param[0].pivot_type == DIFFWHEEL)
    { 
      pivot_value[0].velocity = (pivot_value[0].joint_value[0].velocity + pivot_value[0].joint_value[1].velocity) / 2.0;
      pivot_value[0].position = (pivot_value[0].joint_value[0].position + pivot_value[0].joint_value[1].position) / 2.0;
      DEBUG_OUT_THROTTLE(get_clock(),
        500, "[TRICYCLE] [ODOOMETRY] [DIFFWHEEL] pivot position : " << pivot_value[0].position);
      pivot_value[0].steer_position = pivot_value[0].external_angle;
      DEBUG_OUT_THROTTLE(get_clock(),
        500, "[TRICYCLE] [ODOOMETRY] [DIFFWHEEL] pivot steer position : " << pivot_value[0].steer_position);        
    }
    else if (pivot_param[0].pivot_type == STEERWHEEL)
    {
      // pivot_value[0].steer_position = pivot_value[0].steer_position + pivot_param[0].steer_offset;
      // for (int i=0; i<2; i++) {
      //   if (pivot_param[0].joint_param[i].drive_mode == TRACTION) {
      //     pivot_value[0].velocity = pivot_value[0].joint_value[i].velocity;
      //     pivot_value[0].position = pivot_value[0].joint_value[i].position;
      //   }
      // }
      DEBUG_OUT_THROTTLE(get_clock(),
        500, "[TRICYCLE] [ODOOMETRY] [STEERWHEEL] pivot position : " << pivot_value[0].position);
      DEBUG_OUT_THROTTLE(get_clock(),
        500, "[TRICYCLE] [ODOOMETRY] [STEERWHEEL] pivot steer position : " << pivot_value[0].steer_position);
    }

    //step2: 把不居中的驱动轮变换到中心线,相关值进行计算
    double pivot_angle = pivot_value[0].steer_position,           //中心转换后的角度
           pivot_velocity = pivot_value[0].velocity,              //中心转换后的速度
           pivot_pos_ = pivot_value[0].position;                  //中心转换后的位置
           delta_pivot_pos_.x = (pivot_pos_ - previous_pivot_pos_)*odom_factor; //中心转换后的位置变化量

    if (abs(delta_pivot_pos_.x) > 0.4)                              //过零时忽略
      delta_pivot_pos_.x = 0.0; 
    previous_pivot_pos_ = pivot_pos_;

    if (abs(pivot_angle) > 0.00001 && abs(pivot_param[0].offset_y) > 0.001)
    {
      double temp_angle, atan_value = atan(pivot_param[0].offset_x / pivot_param[0].offset_y);
      if (abs(pivot_angle - PI_2) < 0.0000001 ) //特殊角度为90度
      {
        temp_angle = atan(pivot_param[0].offset_x / pivot_param[0].offset_y);
        if (pivot_param[0].offset_y < 0)
          temp_angle = temp_angle + PI;
      }
      else if (abs(pivot_angle + PI_2) < 0.0000001 )//特殊角度为-90度
      {
        temp_angle = atan(pivot_param[0].offset_x / pivot_param[0].offset_y);
        if (pivot_param[0].offset_y > 0) 
          temp_angle = normAngle2(temp_angle - PI);
      }
      else if (abs(pivot_angle + atan_value) < 0.0000001)//特殊情况可能出现除0错误
        pivot_angle > 0 ? temp_angle = PI_2 : temp_angle = -PI_2;
      else
      {
        temp_angle = atan(pivot_param[0].offset_x / (pivot_param[0].offset_x / tan(pivot_angle) + pivot_param[0].offset_y));
        if (pivot_param[0].offset_y > 0 &&  temp_angle < -atan(pivot_param[0].offset_x / pivot_param[0].offset_y) || 
            pivot_param[0].offset_y < 0 &&  temp_angle < -atan(pivot_param[0].offset_x / pivot_param[0].offset_y) - PI)
          temp_angle = temp_angle - PI;
        
        if (pivot_param[0].offset_y > 0 &&  temp_angle > -atan(pivot_param[0].offset_x / pivot_param[0].offset_y) + PI || 
            pivot_param[0].offset_y < 0 &&  temp_angle > -atan(pivot_param[0].offset_x / pivot_param[0].offset_y))
          temp_angle = temp_angle + PI;
      }
      pivot_velocity = pivot_velocity * sin(pivot_angle) / sin(temp_angle);
      delta_pivot_pos_.x = delta_pivot_pos_.x * sin(pivot_angle) / sin(temp_angle);
      pivot_angle = temp_angle;
    }

    //由驱动单元的坐标转换为车体坐标的计算
    pivot_vel_.x = pivot_velocity;      
    pivot_vel_.y = 0.0;
    pivot_vel_.a = 0.0;
    pivot_vel_in_vehicle_ = frameTrans(pivot_vel_, pivot_angle);
    
    // RCLCPP_INFO_STREAM("pivot_vel x,y,a: " << pivot_vel_.x << ","
    //                           << pivot_vel_.y << "," << pivot_vel_.a);
    // RCLCPP_INFO_STREAM("pivot_vel_in_vehicle : " << pivot_vel_in_vehicle_.x << ","
                              // << pivot_vel_in_vehicle_.y << "," << pivot_vel_in_vehicle_.a);
    delta_pivot_pos_.y = 0.0;
    delta_pivot_pos_.a = 0.0; 
    delta_pos_in_vehicle_ = frameTrans(delta_pivot_pos_, pivot_angle);

    // RCLCPP_INFO_STREAM("delta_pivot_pos x,y,a: " << delta_pivot_pos_.x << ","
    //                           << delta_pivot_pos_.y << "," << delta_pivot_pos_.a);      
    // RCLCPP_INFO_STREAM("delta_pos_in_vehicle : " << delta_pos_in_vehicle_.x << ","
    //                           << delta_pos_in_vehicle_.y << "," << delta_pos_in_vehicle_.a);

    //step3: 计算位置变化量和速度
    Sxya delta_pos;
    double omiga = 0.0, vx = 0.0, vy = 0.0;
    delta_pos.x = delta_pos_in_vehicle_.x;
    delta_pos.y = 0;
    delta_pos.a = delta_pos_in_vehicle_.y / pivot_param[0].offset_x;

    vx = pivot_vel_in_vehicle_.x;
    vy = 0;
    omiga = pivot_vel_in_vehicle_.y / pivot_param[0].offset_x;
    if(standstill){
      delta_pos.x = 0.0;
      delta_pos.y = 0.0;
      delta_pos.a = 0.0;
    }

    DEBUG_OUT_THROTTLE(get_clock(),
      500, "delta_theta :" << delta_pos.a);
    //step4 积分得到车体在odometry坐标的位置
    pos_vehicle_.x += delta_pos.x;
    pos_vehicle_.y += delta_pos.y;
    pos_vehicle_.a += delta_pos.a;
    pos_vehicle_.a = normAngle2(pos_vehicle_.a);

    Sxya delta_pos_map;
    delta_pos_map = frameTrans(delta_pos, position_.a);
    position_.x += delta_pos_map.x;
    position_.y += delta_pos_map.y;
    position_.a += delta_pos_map.a;
    position_.a = normAngle2(position_.a);

    velocity_.x = vx;
    velocity_.y = vy;
    velocity_.a = omiga;

    DEBUG_OUT_THROTTLE(get_clock(),
      500, 
      "position  :" << 
      position_.x << "," << 
      position_.y << "," << 
      position_.a << 
      " velocity :"<< 
      velocity_.x << "," << 
      velocity_.y << ","<< 
      velocity_.a);
  }

  Sxya TricycleOdometry::frameTrans(Sxya original, double angle)
  { 
    Sxya target;
    target.x = original.x * cos(angle) - original.y * sin(angle);
    target.y = original.x * sin(angle) + original.y * cos(angle);
    target.a = original.a;
    return target;
  }
} // namespace agv_tricycle_controller
